
#include "stdafx.h"
#include "kernelprocess.h"
#include "atltime.h"
#include "MFC_ControlDlg.h"
#include "HString.h"
//#include <QTest>

 int Flagone;
 int FlagLocate1;
 int FlagLocate2;
 int FlagLocate3;
 int FlagMove;
 int FlagGrab;
 double FlagPose,FlagPosey;
 extern CMFC_ControlDlg *C;
 Robot R;

KernelProcess::KernelProcess()
//CObject(parent)
{

    //手眼标定参数，Halcon标定得到的参数是手在相机坐标系中的坐标，需要求逆
//    Pose cPt = genPose(0.1157273, 142.5515, 17.4778, 181.382, 0.57467, 269.9901);
//    cv::Mat cTt = MatPose(cPt);
//    tPc = PoseMat(cTt.inv());
    Poff1 = genPose(0,0,0,0,0,0);
    Poff2 = genPose(0,0,0,0,0,0);
    PpreOff2 = genPose(0,0,-100,0,0,0);

//    tPc = genPose(133.083786767065,4.54228389610829,5.37328299175982,
//                  179.92232275093,-0.359640180143151,-89.7340003667202);
//    Pose cPo = genPose(-11.9288,-16.0573,272.2,
//                       10.1019,0.569373,201.322);
//    //    cPo2 = genPose(-11.9456,-15.9409,272.209,
//    //                   10.1677,0.584622,-158.71);

//    wPt = genPose(865.718994140625,-7.49443006515503,544.02099609375,
//                  0.00492110988125205,-1.66636002063751,0.00216553988866508);
//    Poff2 = genPose(0,0,0,0,0,-90);

//    Pose P_go = PoseMat(MatPose(wPt)
//                   *MatPose(tPc)
//                   *MatPose(cPo)
//                   *MatPose(Poff2));

//    P_go = genPose(1030.89001464844,1.26091003417969,269.726013183594,
//                        0.635519027709961,-3.7087299823761,-22.2444000244141);
//    P_go.rx = P_go.rx + 180;
//    P_go.rz = P_go.rz + 180;

//    tPc = PoseMat(MatPose(wPt).inv()
//                  *MatPose(P_go)
//                  *MatPose(Poff2).inv()
//                  *MatPose(cPo).inv());

    //通过反算得到的tPc
//    tPc = genPose(122.878693756372,13.7719997375298,-8.2529104870714,
//                  -176.144228761294,-3.47330508826311,-90.9452855577989);
    //通过标定得到的tPc
//    tPc = genPose(133.083786767065,4.54228389610829,5.37328299175982,
//                  179.92232275093,-0.359640180143151,-89.7340003667202);

    //通过第二次标定得到的tPc
// tPc = genPose(-3.47116101691423,143.006438325705,64.4319114339376,
//               0.807526760226445,0.807316009270384,269.915142191211);


    //起始点，第一次定位点
//   moveAssembly[0]=genMove(764.808,-0.222,498.829,-180.000,-0.000,-0.000,1,40);
     moveAssembly[0]=genMove(800.338,44.853,600.168,0.000,0.000,0.000,1,60);

    //moveAssembly[0]=genMove(800.338,44.853,600.168,0.000,0.000,0.000,1,60);
//    //第二次定位点，
//    moveAssembly[1]=genMove(965.689,-7.50018,544.03,0.00288336,-1.66339,0.00360651,
//                            1,40);
    //预备抓取点
    moveAssembly[1]=genMove(800.338,44.853,550.168,0.000,0.000,0.000,1,60);
    //抓取点
    moveAssembly[2]=genMove(865.689,44.853,344.03,0.00288336,-1.66339,0.00360651,
                            1,40);
    // Next Perform点
     moveAssembly[3]=genMove(62.804,819.594,558.839,0.422,-0.605,92.389,1,120);
   // moveAssembly[3]=genMove(820.338,20.853,600.168,0.000,0.000,0.000,1,60);
    moveAssembly[4]=genMove(850.338,-6.853,600.168,0.000,0.000,0.000,1,60);
    moveAssembly[5]=genMove(880.338,20.853,600.168,0.000,0.000,0.000,1,60);
    moveAssembly[6]=genMove(900.338,44.853,600.168,0.000,0.000,0.000,1,60);
    moveAssembly[7]=genMove(880.338,68.853,600.168,0.000,0.000,0.000,1,60);
    moveAssembly[8]=genMove(850.338,94.853,600.168,0.000,0.000,0.000,1,60);
    moveAssembly[9]=genMove(820.338,68.853,600.168,0.000,0.000,0.000,1,60);
//    //抓住后，先抬起来
//    //起始点，第一次定位点
//    moveAssemblePiston[0]=genMove(802.334,8.235,462.854,179.179,-3.478,4.702,
//                            1,50);

//    //夹具上方旋转（绕y轴）
//    moveAssemblePiston[1]=genMove(-64.7214,-664.656,496.523,-8.25875,-69.7238,-167.274,
//                            1,40);

//    //接近点
//    moveAssemblePiston[2]=genMove(-64.7211,-664.656,264.976,-8.25875,-69.7237,-167.274,
//                            1,40);

//    //非常接近
//    moveAssemblePiston[3]=genMove(-124.338,-664.656,257.488,-8.25876,-69.7238,-167.274,
//                            1,10);

//    //关闭手爪

//    //退出
//    moveAssemblePiston[4]=genMove(-77.298,-664.656,257.488,-8.25869,-69.7238,-167.274,
//                            1,10);
//    //打开手爪

//    //推入
//    moveAssemblePiston[5]=genMove(-93.524,-664.656,257.49,-8.25844,-69.7242,-167.274,
//                            1,10);

//    //关闭夹具
//    //撤出点
//    moveAssemblePiston[6]=genMove(-52.6264,-664.656,257.489,-8.25868,-69.7239,-167.274,
//                            1,10);
//    //抬起
//    moveAssemblePiston[7]=genMove(-52.6264,-664.656,507.105,-8.25865,-69.7239,-167.274,
//                            1,10);
//    //回到原点
//    moveAssemblePiston[8]=genMove(865.689,-7.50018,544.03,0.00288336,-1.66339,0.00360651,
//                            1,40);

    //两次位姿计算的权重

    //
//    //x 和 y方向的权重，第二次定位（小孔）要准一些
//    w1[0] = 0.0;
//    w2[0] = 1.0;
//    w1[1] = 0.0;
//    w2[1] = 1.0;
//    //z 方向定位， 两次定位误差差不多
//    w1[2] = 0.5;
//    w2[2] = 0.5;
//    //rx方向定位， 两次定位误差差不多
//    w1[3] = 0.5;
//    w2[3] = 0.5;
//    //ry方向定位， 第一次定位准一些
//    w1[4] = 0.8;
//    w2[4] = 0.2;
//    //rz 方向定位， 只有第二次定位有效
//    w1[5] = 0;
//    w2[5] = 1;

    // 只是用小圆定位
    //x 和 y方向的权重，第二次定位（小孔）要准一些
    w1[0] = 0.0;
    w2[0] = 1.0;
    w1[1] = 0.0;
    w2[1] = 1.0;
    //z 方向定位， 两次定位误差差不多
    w1[2] = 0.0;
    w2[2] = 1.0;
    //rx方向定位， 两次定位误差差不多
    w1[3] = 0.0;
    w2[3] = 1.0;
    //ry方向定位， 第一次定位准一些
    w1[4] = 0.0;
    w2[4] = 1.0;
    //rz 方向定位， 只有第二次定位有效
    w1[5] = 0.0;
    w2[5] = 1.0;

  //  tmrAssembly = new QTimer(this);

    isRobotMoving = false;
    isAdjusting = false;
    isLocateCorrect = false;
    isAssemblingPiston = false;
	isAssemblingPeg = false;
    }

int KernelProcess::calc_tPc()
{
    return 1;
}

void KernelProcess::onLoadModel()
{
	C-> show_cmd(CString("Loading Model...\r\n"));
 //   cout<<"Loading Model..."<<endl;
    if(-1 == locate1.loadModel())
    C->show_cmd(CString("Model-1 failed, Check the file or Halcon License.\r\n"));
//        cout<<"Model-1 failed, Check the file or Halcon License."<<endl;
     else if(-1 == locate2.loadModel())
    C->show_cmd(CString("Model-2 failed, Check the file or Halcon License.\r\n"));
//        cout<<"Model-2 failed, Check the file or Halcon License."<<endl;
 //    else if(-1 == locate3.loadModel())
 //        cout<<"Model-3 failed, Check the file or Halcon License."<<endl;
 //    else if(-1 == locate4.loadModel())
 //        cout<<"Model-4 failed, Check the file or Halcon License."<<endl;
    else
	C->show_cmd(CString("Loading Model complete.\r\n"));
 //      cout<<"Complete."<<endl;
}

void KernelProcess::onLocate_1()
{
	C->show_cmd(CString("locate begin...\r\n"));

    //Pose cPt = genPose(-2.48973233197427,133.206790065198,43.845740383254
    //                ,180.86817236623,0.818855271282872,268.54466389573);      //手眼标定参数 单位;mm
//	Pose cPt = genPose(-8, 67.4663, 10,1.28,264.28,175.69);      //手眼标定参数 单位;mm
	Pose cPt = genPose(9.3, -94.4, -10.9943, 30.7608, 86.8575, 112.2250);      //手眼标定参数 单位;mm
    tPc = PoseMat(MatPose(cPt).inv());
	CString strTime;                                  // 用于将CTime对象格式化为字符串 
	CTime startTime = CTime::GetCurrentTime();        //获得当前时间
	int nHour = startTime.GetHour();                  // 获取当前小时时间   
	int nMin = startTime.GetMinute();                 // 获取当前分钟时间   
	int nSec = startTime.GetSecond();                 // 获取当前秒时间

   // 只使用小孔进行定位
	C->show_cmd(CString("Locate by a model.\r\n"));
   // timeCount.start();
    if(-1 == locate2.locate(cPo2))
    {   
		C->show_cmd(CString("Locating by model failed.\r\n"));
        return ;
    }

	CTime endTime = CTime::GetCurrentTime();  //获得当前时间
	int nHour2 = endTime.GetHour();  // 获取当前小时时间   
	int nMin2 = endTime.GetMinute(); // 获取当前分钟时间   
	int  nSec2 = endTime.GetSecond(); // 获取当前秒时间
	//string s = "11";
	CString strH, strM, strS;
	// 计算时间差   
	CTimeSpan timeSpan;
	timeSpan = endTime - startTime;
	strH.Format(_T("%d"), timeSpan.GetHours());
	strM.Format(_T("%d"), timeSpan.GetMinutes());
	strS.Format(_T("%d"), timeSpan.GetSeconds());
	C->show_cmd(CString("Complete in: "));
	C->show_cmd(strH);
	C->show_cmd(CString("h "));
	C->show_cmd(strM);
	C->show_cmd(CString("m "));
	C->show_cmd(strS);
	C->show_cmd(CString("s\r\n"));

    cPo = cPo2;    //三维物体相对相机的位姿
	cPoZ = cPo.rz;
 	cPo.rz = 0;
//	P_go2 = PoseMat(MatPose(wPt));	
	//P_go2 = PoseMat(MatPose(p2)*MatPose(p1));
	//P_go2 = PoseMat(MatPose(p3)*MatPose(P_go2));
	//P_go2 = PoseMat(MatPose(wPt)*MatPose(P_go2));
 
	P_go2 = PoseMat(MatPose(wPt)     //手相对于世界坐标（机器人当前位姿）
                    *MatPose(tPc)    //眼相对于手的坐标
                    *MatPose(cPo)
                    *MatPose(Poff2));

	P_go2 = PoseMat(MatPose(genPose(154, -40, -710, 0, 0, 0))*MatPose(P_go2));   

	P_go2.rx = wPt.rx;
	P_go2.ry = wPt.ry;
	P_go2.rz = wPt.rz + 180 - cPoZ;
    //装配角度限制
 //   P_go2.rx = 0.01;
   // P_go2.ry = 0.01;
 //   P_go2.ry = 0.01;
    //P_go2.rz =P_go2.rz -3.5;
//   P_go2.rz =-(P_go2.rz -19);

 //自己注释的
 /*   if(P_go2.rz >90)
        P_go2.rz = P_go2.rz - 180;
    else if(P_go2.rz <-90)
        P_go2.rz = P_go2.rz + 180;
    if(P_go2.rz >=0)
        P_go2.x= P_go2.x-12;
    else if(P_go2.rz <0)
        P_go2.x= P_go2.x-9;*/



    //float zMin = 277.854f;
    //P_go2.z = zMin;


    isLocateCorrect = true;
        isAdjusting = false;
	C->show_cmd(CString("Localization is completed coreectly.\r\n"));   //定位结束


    //Ppre_go2 = PoseMat(MatPose(P_go2)*MatPose(genPose(0,0,100,0,0,0)));
    Ppre_go2.x = P_go2.x;
    Ppre_go2.y = P_go2.y-100;
    Ppre_go2.z = P_go2.z;
    Ppre_go2.rx = P_go2.rx;
    Ppre_go2.ry = P_go2.ry;
    Ppre_go2.rz = P_go2.rz;

    //加入装配控制点序列
    moveAssembly[1] = genMove(Ppre_go2,1,40);
    moveAssembly[2] = genMove(P_go2,1,30);

  // emit locate_1_Complete(cPo);       //在界面显示坐标cPo（相机坐标系中的位姿）
  //  emit Pgo2Calculated(P_go2);        //让机器人运动到点P_go2
	Pose_Togo(Ppre_go2);                  //让机器人运动到定位点P_go2
	Sleep(1000);
//	R.onswitchcylinder1(1);      //手爪张开
	Pose_Togo(P_go2);
	Sleep(3000);
//	R.onswitchcylinder1(0);      //手爪闭合
	Pose_Togo(Ppre_go2);
	Sleep(500);
//	//后续步骤：给固定点，进行自动装配
}

//调整手爪的位姿
void KernelProcess::onAdjust()
{

    CString str;
    numIterate ++;
//    cout<<str.setNum(numIterate)<<" + "<<" th iterating..."<<endl;
//    timeCount.start();

    // 只使用小孔进行定位
	C->show_cmd(CString("Locate by Model 2.\r\n"));
	CTime startTime = CTime::GetCurrentTime();     //获得当前时间
    if(-1 == locate2.locate(cPo2))
    {
		C->show_cmd(CString("Locating by model-2 failed.\r\n"));
        return;
    }
//    cout<<"Complete in ")<<str.setNum(timeCount.elapsed()) "+" <<" ms."<<endl;
 //  emit locateResult1();                          //显示结果图像，需要添加显示图像窗口
	C->show_image();                                 //显示结果图像
    cPo.x = cPo1.x * w1[0] + cPo2.x * w2[0];
    cPo.y = cPo1.y * w1[1] + cPo2.y * w2[1];
    cPo.z = cPo1.z * w1[2] + cPo2.z * w2[2];
    cPo.rx = cPo1.rx * w1[3] + cPo2.rx * w2[3];
    cPo.ry = cPo1.ry * w1[4] + cPo2.ry * w2[4];
    cPo.rz = cPo1.rz * w1[5] + cPo2.rz * w2[5];


//    CalcPose calc;
//    Pose tPo = PoseMat(MatPose(tPc)*MatPose(cPo2));

    //P_go2 = wPt * tPc * cPo2 * Poff2
    P_go2 = PoseMat(MatPose(wPt)
                    *MatPose(tPc)
                    *MatPose(cPo)
                    *MatPose(Poff2));

    //装配角度限制
    P_go2.rx = 0;
    P_go2.ry = 0;
    if(P_go2.rz >90)
        P_go2.rz = P_go2.rz - 180;
    else if(P_go2.rz <-90)
        P_go2.rz = P_go2.rz + 180;

    P_go2.z = P_go2.z < 264 ? 264 : P_go2.z;

    poseOb = genPose(P_go2.x-133,P_go2.y,P_go2.z+280,0,0,0);
    //wTc' = wTo * Toff * tTc^(-1)
    //     = (wTt * tTc * cTo) * Toff * tTc^(-1)
    Pose poseOb2 = PoseMat(MatPose(wPt)
                     *MatPose(tPc)
                     *MatPose(cPo)
                     *MatPose(genPose(0,0,-270,0,0,0))
                     *MatPose(tPc).inv());

//    Pose wPc0 = PoseMat(MatPose(wPt)
//                       *MatPose(tPc));

//    Pose wPo =  PoseMat(MatPose(wPc0)
//                        *MatPose(cPo));

//    Pose wPc = PoseMat(MatPose(wPo)
//                       *MatPose(genPose(0,0,-270,0,0,0)));

//    poseOb2 = PoseMat(MatPose(wPc)
//                           *MatPose(tPc).inv());

    //角度修正
    poseOb2.rx = 0;
    poseOb2.ry = 0;

//    if(poseOb2.rz >90)
//        poseOb2.rz = poseOb2.rz - 180;
//    else if(poseOb2.rz <-90)
//        poseOb2.rz = poseOb2.rz + 180;
	C->show_cmd(CString("Moving robot to new observing point...\r\n"));
	C->show_cmd(CString("pose2Str(poseOb"));
//    emit moveRobot(genMove(poseOb,1,20));
   // emit locateResult1();
  //  emit moveRobot(genMove(poseOb2,1,20));         //控制机器人进行运动
	R.setPose(genMove(poseOb2, 1, 20));              //控制机器人进行运动
    //装配角度修正
//    P_go2.rx = P_go2.rx - 180;
//        P_go2.rz = P_go2.rz - 180;

    //如果两次定位之间差距很小，则表示定位完毕
//    if(distPoseT(cPo0,cPo)<0 && distPoseR(cPo0,cPo)<0)
    if(distPoseT(cPo0,cPo)<8 && distPoseR(cPo0,cPo)<8)

    {
        isLocateCorrect = true;
        isAdjusting = false;
		C->show_cmd(CString("Localization is completed coreectly.\r\n"));
//        emit log(tr("Localization is completed coreectly."));
//        emit locateResult1();                         //显示结果图像，需要添加显示图像窗口
		C->show_image();                                 //显示结果图像
        Ppre_go2 = PoseMat(MatPose(P_go2)*MatPose(genPose(0,0,100,0,0,0)));
        //加入装配控制点序列
        moveAssembly[1] = genMove(Ppre_go2,1,20);
        moveAssembly[2] = genMove(P_go2,1,5);

//        emit locate_1_Complete(cPo);         //在界面显示坐标cPo
//        emit Pgo2Calculated(P_go2);          //让机器人运动到点P_go2  
		Pose_Togo(P_go2);                      //让机器人运动到定位点P_go2
        isLocateCorrect = false;
    }
    else if(numIterate>5)//如果迭代次数超过5次，则定位失败
    {
        isLocateCorrect = false;
        isAdjusting = false;
		C->show_cmd(CString("Can't find the object.\r\n"));
        return;
    }
    cPo0 = cPo;
}

void KernelProcess::onLocate_2() 
{
//    QTime timeCount;
    CString str;
//    img.Save(L"cache.png");
//    timeCount.start();
//	emit log(tr("Locate by Model 3..."));
	C->show_cmd(CString("Locate by Model 3...\r\n"));
    if(-1 == locate3.locate(cPo))   //使用模型3进行定位
    {
		C->show_cmd(CString("Locating by model-3 failed.\r\n"));
//        emit log(tr("Locating by model-3 failed."));
        return ;
    }
//    emit locateResult2();
	C->show_image();
//    emit log(tr("Complete in ")+ str.setNum(timeCount.elapsed()) + tr(" ms."));

    Pose cPt = genPose(-2.48973233197427,133.206790065198,43.845740383254
                    ,180.86817236623,0.818855271282872,268.54466389573);


    tPc = PoseMat(MatPose(cPt).inv());

    P_go2 = PoseMat(MatPose(wPt)
                    *MatPose(tPc)
                    *MatPose(cPo)
                    *MatPose(Poff2));

    //装配角度限制
    P_go2.rx = 0.01;
    P_go2.ry = 0.01;
    P_go2.rz = 0.01;
    P_go2.z = 298.343;

    Ppre_go2 = PoseMat(MatPose(P_go2)*MatPose(genPose(0,0,100,0,0,0)));

    //加入装配控制点序列
    moveAssembly[1] = genMove(Ppre_go2,1,40);
    moveAssembly[2] = genMove(P_go2,1,30);

//    emit locate_1_Complete(cPo);
//    emit Pgo2Calculated(P_go2);
	Pose_Togo(P_go2);
    FlagLocate2 =0;
}

void KernelProcess::onLocate_3()
{
//   QTime timeCount;
    CString str;
//    img.Save(L"cache.png");
//    timeCount.start();
	C->show_cmd(CString("Locate by Model 4...\r\n"));
//    emit log(tr("Locate by Model 4..."));
    if(-1 == locate4.locate(cPo))   //使用模型4进行定位
    {
		C->show_cmd(CString("Locating by model-4 failed.\r\n"));
//        emit log(tr("Locating by model-4 failed."));
        return ;
    }
	C->show_image();
//    emit locateResult3();
//    emit log(tr("Complete in ")+ str.setNum(timeCount.elapsed()) + tr(" ms."));

    Pose cPt = genPose(-2.48973233197427,133.206790065198,43.845740383254
                    ,180.86817236623,0.818855271282872,268.54466389573);


    tPc = PoseMat(MatPose(cPt).inv());

    P_go2 = PoseMat(MatPose(wPt)
                    *MatPose(tPc)
                    *MatPose(cPo)
                    *MatPose(Poff2));
    P_go2 = PoseMat(MatPose(P_go2)*MatPose(genPose(0,0,0,0,0,90))*MatPose(genPose(0,0,0,180,0,0)));
    //装配角度限制
    P_go2.rx = 0.01;
    P_go2.ry = 0.01;
    //P_go2.z = 253.751;
    //P_go2.rz =0.01;
    P_go2.z = 252.789;
    //P_go2.rz =-(P_go2.rz -3.5);
    P_go2.rz =-P_go2.rz;
    if(P_go2.rz >90)
        P_go2.rz = P_go2.rz - 180;
    else if(P_go2.rz <-90)
        P_go2.rz = P_go2.rz + 180;


    Ppre_go2 = PoseMat(MatPose(P_go2)*MatPose(genPose(0,0,100,0,0,0))*MatPose(genPose(0,30,0,0,0,0)));
    //Ppre_go2 = PoseMat(MatPose(P_go2)*MatPose(genPose(0,0,100,0,0,0)));
    P_go2.x = Ppre_go2.x;
    P_go2.y = Ppre_go2.y;
    //加入装配控制点序列
    moveAssembly[1] = genMove(Ppre_go2,1,40);
    moveAssembly[2] = genMove(P_go2,1,30);

//    emit locate_1_Complete(cPo);
//    emit Pgo2Calculated(P_go2);
	Pose_Togo(P_go2);
    //FlagLocate2 =0;
    FlagLocate3 =0;
}

void KernelProcess::onImageReceive(CImage *img)
{
    this->img = *img;
    if(isAdjusting && !isRobotMoving)//机器人正在运动的时候不做处理
    onAdjust();
}

//接收机器人位姿数据
void KernelProcess::onRobotPoseReceive(Pose p)
{
    wPt = p;
}



void KernelProcess::onSwitchVavle2(int status)
{
//    rbt->switchVavle2(status);
}
void KernelProcess::onSwitchHand(int status)
{
//    rbt->switchHand(status);
}
//接收机器人运动信息（机器人是否在运动）
void KernelProcess::onIsRobotMoving(bool s)
{
    isRobotMoving = s;

}

//抓取活塞后，启动控制机器人送活塞操作
//void KernelProcess::onPbGoAssemblyPiston()
//{
//    stepAssemblePiston = 0;
//    onTmrAssemblePiston();
// //   connect(tmrAssembly,SIGNAL(clicked()),this,SLOT(onTmrAssemblePiston()));
//    if(!tmrAssembly->isActive())
//        tmrAssembly->start(400);

//}
/*
//抓取销子后，启动控制机器人送销子操作
void KernelProcess::onPbGoAssemblyPeg()
{
//    stepAssemblePeg = 0;
//    connect(tmrAssembly,SIGNAL(timeout()),this,SLOT(onTmrAssemblePeg()));
//    if(!tmrAssembly.isActive())
//        tmrAssembly.start(100);
	
    emit log(tr("Inserting the Peg"));
//    QString program = "D:\\Projects\\Qt Projects\\RRD_release\\RobotRemoteDebugger.exe";
    QString program = "InsertAPeg.exe";
    QStringList arguments;
    arguments << "";
    QProcess *myProcess = new QProcess(this);
    myProcess->start(program, arguments);

}
//抓连杆，放在台子上
void KernelProcess::onPbGoAssemblyLine()
{

    emit log(tr("Inserting the Line"));
//    QString program = "D:\\Projects\\Qt Projects\\RRD_release\\RobotRemoteDebugger.exe";
    QString program = "InsertLine.exe";
    QStringList arguments;
    arguments << "";
    QProcess *myProcess = new QProcess(this);
    myProcess->start(program, arguments);

}

int KernelProcess::onPbGrabunion()
{
    emit log(tr("Grab the piston and the line"));
//    QString program = "D:\\Projects\\Qt Projects\\RRD_release\\RobotRemoteDebugger.exe";
    QString program = "union.exe";
    QStringList arguments;
    arguments << "";
    QProcess *myProcess = new QProcess(this);
    myProcess->start(program, arguments);
    return 0;


}

void KernelProcess::onPbGoAssemblyPiston()
{
    emit log(tr("Inserting the piston"));
//    QString program = "D:\\Projects\\Qt Projects\\RRD_release\\RobotRemoteDebugger.exe";
    QString program = "InsertAPiston.exe";
    QStringList arguments;
    arguments << "";
    QProcess *myProcess = new QProcess(this);
    myProcess->start(program, arguments);


}

int KernelProcess::onPbGrabtest()
{
    emit log(tr("Grab the piston and the line"));
//    QString program = "D:\\Projects\\Qt Projects\\RRD_release\\RobotRemoteDebugger.exe";
    QString program = "control.exe";
    QStringList arguments;
    arguments << "";
    QProcess *myProcess = new QProcess(this);
    //connect(myProcess, SIGNAL(finished(int)), SLOT(processFinished(int exitCode))); // 自己绑定槽吧
    myProcess->execute(program, arguments);
    //return myProcess->exitCode(); 这句没有意义
    return 0;
}

void KernelProcess::processFinished(int exitCode)
{ // 这个函数参数一定要跟QProcess::finished一致
    // exitCode这个是某进程退出时的返回值
    emit changeFlag();
}

//定时器，装配活塞
void KernelProcess::onTmrAssemblePiston()
{


    //调用外部程序
    emit log(tr("Inserting the Piston"));
    QString program = "InsertAPiston.exe";
    QStringList arguments;
    arguments << "";
    QProcess *myProcess = new QProcess(this);
    myProcess->start(program, arguments);

}
//定时器，装配销子
void KernelProcess::onTmrAssemblePeg()
{

}
*/
void KernelProcess::Pose_Togo(Pose P)
{
	char recvMsg[256] = "";
	float flXyzAboveGripper4[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 0.000 };
	sendCmd(ControlRobot_SetSpeed(3), recvMsg);                    // 首先设置机器人速度
	flXyzAboveGripper4[0] = P.x;
	flXyzAboveGripper4[1] = P.y;
	flXyzAboveGripper4[2] = P.z;
	flXyzAboveGripper4[3] = P.rx;
	flXyzAboveGripper4[4] = P.ry;
	flXyzAboveGripper4[5] = P.rz;
	sendCmd(ControlRobot_SetXyzwpr(flXyzAboveGripper4), recvMsg);
}
